#include "AStarSolver.h"
#include "qdebug.h"
#include<QTime>
#include<QCoreApplication>
#include <QFileInfo>

void delayMSec(unsigned int msec)
{
    QTime Time_set = QTime::currentTime().addMSecs(msec);
    while( QTime::currentTime() < Time_set )
        QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
}

AStarSolver::AStarSolver()
{
    m_hMethod = Manhattan;
    // initMap();
    mode = AUTO;
    state = FREE;
    current_best = Location{-1,-1};
}

void AStarSolver::search(Location start, Location goal)
{
    // 清空数据结构
    if(state!=FREE) return;
    came_from.clear();
    cost_so_far.clear();
    cost_heuristic.clear();
    cost_total.clear();
    frontier.clear();
    path.clear();

    // 初始化
    state = RUNNING;
    frontier.append(Loc_with_cost(0, start));
    came_from[start] = start;
    cost_so_far[start] = 0;

    // 循环搜索
    while (!frontier.empty())   // OPEN表不为空时
    {
        current = frontier.first().second;
        frontier.pop_front();

        if (current == goal)    // 找到终点
        {
            emit step();    // 给主窗口发信号
            stop();         // 停止搜索
            break;
        }

        neighbors = getNeighbors(current);

        for (int i = 0; i < neighbors.count(); ++i)
        {   // 对每个邻居：
            Location next = neighbors[i];
            float new_cost = cost_so_far[current] + cost(current, next);    // 计算当前实际代价g(n)
            if (cost_so_far.find(next) == cost_so_far.end() ||
                new_cost < cost_so_far[next])   // 不在CLOSE表中，或新的代价更小
            {
                cost_so_far[next] = new_cost;   // 更新实际代价
                cost_heuristic[next] = heuristic(next, goal);   // 计算启发估计代价
                double total = new_cost + cost_heuristic[next]; // 计算总代价
                cost_total[next] = total;
                frontier.append(Loc_with_cost(total, next));    // 加入OPEN表
                came_from[next]=current;    // 记录路径信息
            }
        }
        // 列表排序
        std::sort(frontier.begin(),frontier.end());

        // 控制信息，与主窗口交互
        if(frontier.empty()) break;
        current_best = frontier.first().second;
        emit step();
        if(mode==AUTO){
            state = SLEEPING;
            delayMSec(50);
            state = RUNNING;
            if(m_stop) {state=FREE; m_stop=false; return;}
        }
        else{
            state = WAITING;
            while(state!=EXECUTE){
                if(m_stop) {state=FREE; m_stop=false; return;}
                QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
            }
        }
    }

    // 解算路径
    path = reconstruct_path(start,goal);
    state = FREE;
}

void AStarSolver::search(int startx, int starty, int goalx, int goaly)
{
    search(Location{startx, starty}, Location{goalx, goaly});
}

void AStarSolver::stop()
{
    if(state!=FREE)
        m_stop = true;
}

float AStarSolver::heuristic(Location a, Location b)
{
    if (m_hMethod == Manhattan)
        return std::abs(a.x - b.x) + std::abs(a.y - b.y);
    else if (m_hMethod == Euclidean)
        return std::pow(((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y)),
                        0.5);
    else
        qCritical() << "错误：未定义的启发函数！";
    return -1;
}

QVector<Location> AStarSolver::getNeighbors(Location loc)
{
    QVector<Location> neibors;
    foreach (auto dir, DIRS)
    {
        Location neibor{loc.x + dir.x, loc.y + dir.y};
        if (in_bounds(neibor) && passable(neibor)) neibors.append(neibor);
    }
    return neibors;
}

float AStarSolver::cost(Location a, Location b)
{
    switch(std::abs(a.x - b.x) + std::abs(a.y - b.y)){
        case 1:return 1.0f;
        case 2:return 1.4f;
    }
    return pow(pow(a.x - b.x, 2) + pow(a.y - b.y, 2), 0.5);
}

bool AStarSolver::in_bounds(Location id)
{
    return 0 <= id.x && id.x < width() && 0 <= id.y && id.y < height();
}

bool AStarSolver::passable(Location id) {
    if(mapAt(id.x, id.y) != ROAD) return false;
    // 检测墙角
    if(current.x==id.x||current.y==id.y) return true;
    if(mapAt(current.x,id.y)!=ROAD && mapAt(id.x,current.y)!=ROAD) return false;
    return true;
}

int AStarSolver::mapAt(int x, int y)const { return map[y][x]; }

void AStarSolver::readMap(QString map_path)
{
    QFileInfo fi(map_path);
    if(!fi.isFile()) return;
    QFile mapFile(map_path);
    if(mapFile.open(QFile::ReadOnly)){
        QTextStream in(&mapFile);
        int row = 0;
        while (!in.atEnd()) {
            QString     line    = in.readLine();
            map.append(QVector<int>());
            QStringList squares = line.split(",");
            foreach (QString sq, squares) {
                map[row].append(sq.toInt());
            }
            row++;
        }
    }
    qInfo()<<"read done.";
}

void AStarSolver::initMap()
{
    map.append(QVector<int>() << 1 << 1 << 1 << 1 << 1);
    map.append(QVector<int>() << 1 << 0 << 0 << 0 << 1);
    map.append(QVector<int>() << 1 << 0 << 0 << 0 << 1);
    map.append(QVector<int>() << 1 << 0 << 1 << 0 << 1);
    map.append(QVector<int>() << 1 << 0 << 0 << 0 << 1);
}

void AStarSolver::printMap()
{
    qInfo() << "map:-----x----->";
    foreach (auto vec, map)
    {
        qInfo() << vec;
    }
}

QVector<Location> AStarSolver::reconstruct_path(Location start, Location goal)
{
    QVector<Location> path;
    Location current = goal;
    if(came_from.find(goal)==came_from.end()){
        return path;
    }
    while(!(current==start)){
        path.push_back(current);
        current = came_from[current];
    }
    path.push_back(start);
    std::reverse(path.begin(),path.end());
    return path;
}

QDebug& operator<<(QDebug& out, const Location& loc)
{
    out << "(" << loc.x << "," << loc.y << ")";
    return out;
}
